{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 70,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Q-Table One"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Q-table One is used to void the obstacles automatically.\n",
    "### Columns:Nearest|Near|Medium|Far\n",
    "##### Columns register the states\n",
    "### Rows:Up|Down|Turn_left_45 degree|Turn_right_45_degree\n",
    "##### Rows register the actions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120])\n",
    "Robot_radium=40\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Safe distance=1cm\n",
    "Q_table1_states=np.array(['L0R0U0','L0R0U1','L0R0U2','L0R0U3',\n",
    "                'L0R1U0','L0R1U1','L0R1U2','L0R1U3',\n",
    "                'L0R2U0','L0R2U1','L0R2U2','L0R2U3',\n",
    "                'L0R3U0','L0R3U1','L0R3U2','L0R3U3',\n",
    "                'L1R0U0','L1R0U1','L1R0U2','L1R0U3',\n",
    "                'L1R1U0','L1R1U1','L1R1U2','L1R1U3',\n",
    "                'L1R2U0','L1R2U1','L1R2U2','L1R2U3',\n",
    "                'L1R3U0','L1R3U1','L1R3U2','L1R3U3',\n",
    "                'L2R0U0','L2R0U1','L2R0U2','L2R0U3',\n",
    "                'L2R1U0','L2R1U1','L2R1U2','L2R1U3',\n",
    "                'L2R2U0','L2R2U1','L2R2U2','L2R2U3',\n",
    "                'L2R3U0','L2R3U1','L2R3U2','L2R3U3',\n",
    "                'L3R0U0','L3R0U1','L3R0U2','L3R0U3',\n",
    "                'L3R1U0','L3R1U1','L3R1U2','L3R1U3',\n",
    "                'L3R2U0','L3R2U1','L3R2U2','L3R2U3',\n",
    "                'L3R3U0','L3R3U1','L3R3U2','L3R3U3'])\n",
    "Q_table1_actions=np.array(['Up','Down','Left_45D','Right_45D'])\n",
    "Q_table1_actions_length=len(Q_table1_actions)\n",
    "Q_table1_states_length=len(Q_table1_states)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 67,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Down</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L0R0U0</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U1</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U2</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R0U3</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L0R1U0</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "         Up  Down  Left_45D  Right_45D\n",
       "L0R0U0  0.0   0.0       0.0        0.0\n",
       "L0R0U1  0.0   0.0       0.0        0.0\n",
       "L0R0U2  0.0   0.0       0.0        0.0\n",
       "L0R0U3  0.0   0.0       0.0        0.0\n",
       "L0R1U0  0.0   0.0       0.0        0.0"
      ]
     },
     "execution_count": 67,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q_table_real=np.zeros((Q_table1_states_length,Q_table1_actions_length))\n",
    "Q_table_real=pd.DataFrame(Q_table_real,columns=Q_table1_actions,index=Q_table1_states)\n",
    "Q_table_real.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 68,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Initial_Q_Table(LengthOfActions,LengthOfStates):\n",
    "    Q_Table=np.zeros((LengthOfStates,LengthOfActions))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAAJCCAYAAAD3HAIiAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x16e72f26128>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def Draw_map1():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255)) \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((1500-20,300-20,1500+20,300+20), (255,0,0), (255,0,0))\n",
    "    return im\n",
    "im_show=Draw_map1()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAAJCCAYAAAD3HAIiAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzt3W+MbPdd3/HPt74QIUpKaGjk2qZxJAfJqSq3ttxIJYiK\nQkxEk0Cl1FFF0jaKQUlRUVuhpEgl6qMWSCtFCCNTrCQVTTBtQ/yAtCQRIk9qghO58R8Scp0/iq+M\nrRKpbgVysfPrgz1rz6733t3Zmd/M+fN6Sas79+zO7G/OnDnz3t+ZP9VaCwAAffy5fQ8AAGDOxBYA\nQEdiCwCgI7EFANCR2AIA6EhsAQB0tPPYqqrbquoLVXWxqt61698PALBLtcv32aqqq5L8YZIfSPJY\nkt9P8ubW2iM7GwQAwA7tembr1iQXW2tfaq39vyQfTvKGHY8BAGBnLuz4912T5Gsr/38syd88/kNV\ndUeSO4b/3ryDcQEAPKe1Vtu6rF3H1pm01u5KcleSVJXPEwIAJmvXhxEvJblu5f/XDssAAGZp17H1\n+0luqKrrq+qbk9ye5N4djwEAYGd2ehixtfZMVf2TJP89yVVJ7m6tPbzLMQAA7NJO3/rhPDxnCwDY\ntW0+Qd47yAMAdCS2AAA6ElsAAB2JLQCAjsQWAEBHo3wH+fMa+ysrAYD9qdraCwzXYmYLAKAjsQUA\n0JHYAgDoSGwBAHQktgAAOhJbAAAdiS0AgI7EFgBAR2ILAKAjsQUA0JHYAgDoSGwBAHQktgAAOrqw\n7wEs1Vk+eby1toORAAA9ia0tO0tErXNZggsApk1srWGbIbXO7xRcXI4ZUoDxE1traK3tJbjg0Hm2\nv9XzCC+A3RNbE2B2a9m2fWj6kG0KYDe8GnEizKgtT1V1vd17Xz4AB8QWjNAuI0h0AfQltta0z0Mv\nHhDnb5/hY/sC6ENsTYjn2MzbGGLHLBfA9oktGAGBAzBfXo14Dvt4CwizWvM1xtA6HJPtDqZnjPuU\nbZnqPsnM1gRMdePidGPfKY59fABTILZGTmjNl5ABWAaxNWJCa76mFFpTGivAGIkt2LEpxssUxwww\nFmLrnHrPOpnVYmwEF8D5iK0RElrzJVgAlkdsjYzQYszEIsD6xNaICK15EyoAyyS2NrDNOBJaTIVo\nBFiP2IIdECgAyyW2RsCsFgDMl9ja0KahJLSYIjN1AGcntvZIaAHA/F3Y9wDmQDRxJWaBAJbNzBYA\nQEdiCwCgI7EFANCR2AIA6EhsAQB0JLYAADoSWwAAHYktAICOxBYAQEdiCwCgI7EFnfk4J4BlE1sA\nAB2JLWBtZusAzk5sAQB0JLZgB8wEASzXuWOrqq6rqt+pqkeq6uGq+qfD8vdU1aWqemD4et3Ked5d\nVRer6gtV9dptXAFgt4QjwHrqvDvOqro6ydWttc9W1bcl+UySNyZ5U5L/21r7hWM/f2OSDyW5Nclf\nTvKJJK9srT17yu858wA9CDB2VbXvIWzM/QzGbQ77mcvZdP+zzrpprW1tRZ57Zqu19nhr7bPD6f+T\n5A+SXHOFs7whyYdba0+31r6c5GIOwguYCKEFsL6tPGerql6e5K8n+b1h0U9W1eeq6u6qesmw7Jok\nX1s522O5TJxV1R1VdX9V3b+N8cFYiBXOo6pO/QLGa+PYqqo/n+S/JPmp1tpTSe5M8ookNyV5PMl7\n173M1tpdrbVbWmu3bDo+YDuE4m6cN6QEGIzXRrFVVd+Ug9D6tdbaf02S1toTrbVnW2vfSPIref5Q\n4aUk162c/dphGSzKFKNlimOemh6BJLpgHDZ5NWIl+dUkf9Ba+3cry69e+bEfSfLQcPreJLdX1Yuq\n6vokNyT59Hl/P0yZeGFV7yASXLBfFzY4799K8mNJHqyqB4Zl/zLJm6vqpiQtyVeS/HiStNYerqp7\nkjyS5Jkk7zztlYgwZ621STwICsN+dnn7H/4utyfs3rnf+mFXvPUDczfm4HKf6mMMt7nbdr7GsH31\nsri3fgC2Y6wPemMd19SN5YFwLOOAJRBbMAJjC5uxjWcuxhY4YxsPzJXYgpEYQ+C01kYxjjkaa9iM\ndVwwJ2ILRmSfoSOy+hl70Ix9fDB1YgtGZtezS2az+ppKyExlnDBFYgtGqncEiaz+phYwUxsvTMUm\n77MF7MBhEG3rgVBgAeyW2IKJOCmSzhJg4mo/pjpLVFW2GdgysbUnU90Rj83SHxSWfv3Haur3b8EF\n2+U5WwAAHYktAICOxBbAFk39EOKhuVwPGAOxBcCJBBdsh9gC2BJxApxEbAEAdCS2AAA68j5bADAj\n3iNtfMxsAWzBXJ+vNdfrBbsktgAAOhJbAAAdiS0AgI7EFgBAR2ILAKAjsQUA0JHYAgDoSGwBAHQk\ntgC2YK7v2j3X6wW7JLYAADoSWwAAHYktgC1xyA04idgC4ETiEbZDbAEAdCS2ALZoLrNBc7keMAZi\nCwCgI7EFsGVTnxWa+vhhbMQWAEBHYgugg6nODk113DBmYgsAoCOxBdDJ1GaJpjZemAqxBdDRVAJm\nKuOEKRJbAJ2NPWTGPj6YOrEFsANjDZqxjgvmRGwB7MjYwmZs44G5ElsAOzSWwBnLOGAJLux7AABL\ncxg6VbW33w3sjtgC2JNdRpfIgv0RWwB71jO6RBbsn9gCGInVMNo0vEQWjIfYAhihk2LppAATVTB+\nYgtgIoQVTJO3fgAA6EhsAQB0JLYAADoSWwAAHYktAICOxBYAQEdiCwCgI7EFANCR2AIA6Gij2Kqq\nr1TVg1X1QFXdPyz7jqr6eFV9cfj3JSs//+6qulhVX6iq1246eACAsdvGzNbfbq3d1Fq7Zfj/u5J8\nsrV2Q5JPDv9PVd2Y5PYkr0pyW5JfqqqrtvD7AQBGq8dhxDck+cBw+gNJ3riy/MOttadba19OcjHJ\nrR1+PwDAaGwaWy3JJ6rqM1V1x7DsZa21x4fTf5TkZcPpa5J8beW8jw3LXqCq7qiq+w8PTQLAnFXV\nWl9My4UNz/89rbVLVfWXkny8qj6/+s3WWquqtT+mvrV2V5K7kuQ85weAsdpGLJ10Ga15uByrjWa2\nWmuXhn+fTPKRHBwWfKKqrk6S4d8nhx+/lOS6lbNfOywDgFnbxayUma/xOndsVdW3VtW3HZ5O8oNJ\nHkpyb5K3Dj/21iQfHU7fm+T2qnpRVV2f5IYknz7v7weAsdtX/IiucdnkMOLLknxkuDEvJPlPrbX/\nVlW/n+Seqnpbkq8meVOStNYerqp7kjyS5Jkk72ytPbvR6AFgZMYUOatjcZhxf2rsK3+d52yN/bqs\nGtOdccqmdJsD8zaF/frS95nr3Eatta3doN5BHgA2NIXQShxe3BexBQDnNNV4meKYp0xsAcA5TD1Y\nphqKUyS2AGBNc4qUOV2XsRJbALCGOcbJHK/TmIgtADijOUfJnK/bvoktADiDJcTIEq7jPogtADjF\nkiJkSdd1Vzb9IGqAnVj3AWDpb97I9iwxPqrKfWiLxBYwaud9oPMxJWzDEkPrkODaHocRgVHa5nsA\neT8hzsM2Yx1si9gCRqVnGHng4KxsK8+zLjYntoDR2MVO3SwXsGtiCxiFXQeQ4OJybBsvZJ1sRmwB\ne7evHbkHEI6zTdCD2AL2at8Pbvv+/TAV7ivnJ7YAIGLiLKyj8xFbwN6MZcc9lnEA8yS2gL0QOMBS\niC2AiL+lc/ufnXW1PrEF7NxYd9ZjHRcwbT4bcU983hTAOIjs9fncxPWY2QIA6EhsATtlFgFYGrEF\nsEIMLovb+/ysu7MTWwAAHYktAICOxBYAQEdiCwCgI7EFwCJ5gvfmrMOzEVsAAB2JLQCAjsQWwAof\nQQJsm9gCdkrMAEsjtgAAOhJbAAAdiS2AgUOcQA9iC9g5UQMsidgC9mJswTW28QDzIbaAxRNaQE9i\nC9gbkQMsgdgC9mrfwbXv38/+uO03Zx2ejdgC9m5fO2wPFMAuiC1gFHYdPkIL2BWxBYzGrgJIaAG7\nJLaAUekZQq01oQXsnNgCRmfbUSSyuBzbxflZd2d3Yd8DALic1Z15VW10foB9EVtM0kkPvB5Y5+34\n7WsbgP1xX1uP2GLU1pnN8OC7LG5btqW1dq6ZUzgrscUobWvHd3g5HpgB2Bexxaj0+uty9XKFF3Cc\n2a2zsw9dn9hiFHa5kzPbBcAueesH9qqq9vbXpL9iAdgFscXejCF2xjAGYBzMdp/OOjofscVejCly\nxjQWYL/ExOVZN+cntti5McbNPg9nAjBvYoudEjTA2JnBeSHrZDNii52ZQmhNYYxAf+LiedbF5s4d\nW1X13VX1wMrXU1X1U1X1nqq6tLL8dSvneXdVXayqL1TVa7dzFZiCKUXMlMYKwPjVNoq1qq5KcinJ\n30zyj5L839baLxz7mRuTfCjJrUn+cpJPJHlla+3ZUy77zANU3+M01XixPQFT3X9ty9z2g+vcnq21\nrd342zqM+P1JHm2tffUKP/OGJB9urT3dWvtykos5CC8YpaXvZIH5xcY6lnzdt21bsXV7DmatDv1k\nVX2uqu6uqpcMy65J8rWVn3lsWPYCVXVHVd1fVfdvaXzsiWABpm6J0bHE69zTxrFVVd+c5PVJfmNY\ndGeSVyS5KcnjSd677mW21u5qrd3SWrtl0/EBwKaWFB9Luq67so2ZrR9K8tnW2hNJ0lp7orX2bGvt\nG0l+Jc8fKryU5LqV8107LGOm5jCrNYfrAGzHEiJkCddxH7YRW2/OyiHEqrp65Xs/kuSh4fS9SW6v\nqhdV1fVJbkjy6S38fgDYiTnHyJyv275d2OTMVfWtSX4gyY+vLP65qropSUvylcPvtdYerqp7kjyS\n5Jkk7zztlYhM15xmhKrKTgh4TmttVvu4RGj1tpW3fujJWz9Mkx0RMHdz2c8taf829bd+AIBFmXqk\ntNYmfx2mQmzBGczlL1hgu6YaLFMc85Rt9JwtOIkwAZbmMF7Gvv8TWfthZgsAtmTMMTPmsc2dmS0A\n2KLVqNn3TJfAGgexBQCd7OPwosAaH7EFAJ3tYrZLZI2X2AKAHTopitYNMGE1LWILAPZMPM2bVyMC\nAHQktgAAOhJbAAAdiS0AgI7EFgBAR2KLrfOqGgB4ntiCMxCQAJyX2AIA6Ehs0YWZIAA4ILbgFMIR\ngE2ILboRKQAgtuCKBCMAmxJbdCVWAFg6sQWXIRQB2AaxRXdTjJYpjhmAcRJb7IR4AWCpxBYcIwwB\n2Caxxc5MIWKmMEYApkVssVOttdEGzVjHBcC0iS2I0AKgH7HFXoxphmss4wBgnsQWe7XP0BlT8AEw\nX2KLvdtH8IgsAHblwr4HAMnR+KmqnfweANgFscXoHAbRtqJLYAGwT2KL0dpktktgATAWYotJEE8A\nTJUnyAMAdCS2AAA6ElsAAB2JLQCAjsQWAEBHYgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA6ElsAAB2J\nLQCAjsQWAEBHYgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA6ElsAAB2JLQCAjsQWAEBHYgsAoCOxBQDQ\n0amxVVV3V9WTVfXQyrLvqKqPV9UXh39fsvK9d1fVxar6QlW9dmX5zVX14PC991VVbf/qAACMy1lm\ntt6f5LZjy96V5JOttRuSfHL4f6rqxiS3J3nVcJ5fqqqrhvPcmeTtSW4Yvo5fJgDA7JwaW621TyX5\n+rHFb0jygeH0B5K8cWX5h1trT7fWvpzkYpJbq+rqJC9urd3XWmtJPrhyHgCA2bpwzvO9rLX2+HD6\nj5K8bDh9TZL7Vn7usWHZnw2njy8/UVXdkeSOc44NYBSqKgd/XzJGu3w2i+1g2c4bW89prbWq2upW\n1Fq7K8ldSbLtywboxVNRgZOc99WITwyHBjP8++Sw/FKS61Z+7tph2aXh9PHlAJNWVc99Xe77wLKd\nN7buTfLW4fRbk3x0ZfntVfWiqro+B0+E//RwyPGpqnr18CrEt6ycB2BSTgssgFWnHkasqg8l+b4k\nL62qx5L8bJJ/k+Seqnpbkq8meVOStNYerqp7kjyS5Jkk72ytPTtc1Dty8MrGb0nyseELYBKEFXBe\nNfYn7a3znK2xXxdgWrYZWPZP4+MJ8suzzm3eWtvaBrLxE+QB5sQMFrBtYgtYPIEF9CS2gMURV8Au\n+SBqYDH2+QpCgQfLZWYLmC2BA4yB2AJmRWABYyO2gMkTWMCYiS1gkqYYWD6YGpZJbAGTMMW4Aki8\nGhEYubl9BuGcrgtwNma2gFERI8DciC1g7wQWMGdiC9g5cQUsiedsATsltKwDWBqxBeyUtz4AlkZs\nATvXWhNdwGKILWBvlhxcDiXCcogtYK+WHFzAMogtYO8EFzBnYgsYhakH1+Hz0NZ5PppDibAM3mcL\nGI3W2mwC5HLBNZfrB5yd2Fox553g1GcNWI7DbXVK98d17l/ui+PhtmBXHEYEAOhIbAGjNJX34prC\nGIH9ElvAqIkZYOrEFjB6Yw2usY4LGBexBUyCsAGmSmwBkzGm4BrTWIBxE1vApIgcYGrEFjA5U3ml\nIkAitoAJ21dwCT1gHWILmDThA4yd2AImb5fBJe6AdYktYBZEEDBWYguYjd7BJeiA8xBbwKx4pSIw\nNmILmKVtB5eAA85LbAGzJZCAMRBbwKxtI7hEG7AJsQXMnlgC9klsAYsguIB9EVvAYpwnuEQasCmx\nBSyKt4YAdk1sAYt0luASZcA2iC1gscQUsAtiC1i0ywWXEAO2RWwBiyesgJ7EFkCOBpf4Arbpwr4H\nALxQVe17CKPVM4REFtCDmS0AgI7EFgBAR2ILAKAjsQUA0JHYAgDoSGwBAHQktgAAOhJbAAAdiS0A\ngI7EFgBAR6fGVlXdXVVPVtVDK8t+vqo+X1Wfq6qPVNW3D8tfXlV/WlUPDF+/vHKem6vqwaq6WFXv\nK59HAgAswFlmtt6f5LZjyz6e5K+21v5akj9M8u6V7z3aWrtp+PqJleV3Jnl7khuGr+OXCQAwO6fG\nVmvtU0m+fmzZb7fWnhn+e1+Sa690GVV1dZIXt9buawef9PrBJG8835ABAKZjG8/Z+sdJPrby/+uH\nQ4i/W1WvGZZdk+SxlZ95bFh2oqq6o6rur6r7tzA+AIC9ubDJmavqZ5I8k+TXhkWPJ/mu1tofV9XN\nSX6zql617uW21u5KctfwO9omYwQA2Kdzx1ZV/cMkP5zk+4dDg2mtPZ3k6eH0Z6rq0SSvTHIpRw81\nXjssAwCYtXMdRqyq25L8dJLXt9b+ZGX5d1bVVcPpV+TgifBfaq09nuSpqnr18CrEtyT56MajBwAY\nuVNntqrqQ0m+L8lLq+qxJD+bg1cfvijJx4d3cLhveOXh9yb511X1Z0m+keQnWmuHT65/Rw5e2fgt\nOXiO1+rzvAAAZqmGI4Cjtc5ztja9LnN+66+x384cNedtcVO2ZeC81tm3tta2tiP2DvIAAB2JLQCA\njsQWAEBHYgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA62uiDqOfGmyUCANtmZgsAoCOxBQDQkdgCAOhI\nbAEAdCS2AAA6ElsAAB2JLQCAjsQWAEBHYgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA6ElsAAB2JLQCA\njsQWAEBHYgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA6ElsAAB2JLQCAjsQWAEBHYgsAoCOxBQDQ0YV9\nDwCAZaiqrVxOa20rlwO7IrYA6GJbcXXa5Yovxk5sAbA1vQLrrL9TeDFGYguAc9tHXF2JWS/GSGwB\nsLaxRdblHI5TdLFPYguAM5tKZB0nutgnsQXAqaYaWceJLvZBbAFwWXOJrONEF7vkTU0BONFcQ2vV\nEq4j+2dmC4AjlhYgZrnozcwWAEkOomNpobVqydedvsQWAEJjsPTgpA+xBbBw4uKFrBO2SWwBLJio\nuDzrhm0RWwALJSZOZx2xDWILYIFExNlZV2zKWz/ACHkJOj2Jh/VVlfsl52ZmC2BBhNb5WXecl9gC\nWAixsDnrkPMQWwALIBK2x7pkXWILYObEwfZZp6xDbAEAdCS2AGbKR8/0Zd1yVmILAM5JcHEWp8ZW\nVd1dVU9W1UMry95TVZeq6oHh63Ur33t3VV2sqi9U1WtXlt9cVQ8O33tf2UIBurGLhfE4y8zW+5Pc\ndsLyf99au2n4+q0kqaobk9ye5FXDeX6pqq4afv7OJG9PcsPwddJlAsCkCFtOc2pstdY+leTrZ7y8\nNyT5cGvt6dbal5NcTHJrVV2d5MWttfvawVvwfjDJG887aAAuz4P/7lnnXMkmz9n6yar63HCY8SXD\nsmuSfG3lZx4bll0znD6+/ERVdUdV3V9V928wPoDF8aAP43Pe2LozySuS3JTk8STv3dqIkrTW7mqt\n3dJau2WblwsAvQhdLudcsdVae6K19mxr7RtJfiXJrcO3LiW5buVHrx2WXRpOH18OwJZ4sIdxOlds\nDc/BOvQjSQ5fqXhvktur6kVVdX0Ongj/6dba40meqqpXD69CfEuSj24wbgAYHcHLSS6c9gNV9aEk\n35fkpVX1WJKfTfJ9VXVTkpbkK0l+PElaaw9X1T1JHknyTJJ3ttaeHS7qHTl4ZeO3JPnY8AUAMGt1\n8OLA8aqqMw9w7NcFoBczKuPi8Wic1rmftNa2dqfyDvIAAB2JLYCJM6s1Pm4TVoktAICOxBbAhJlB\ngfETWwDQgRDmkNgCAOhIbAEAdCS2ACbKYarxcxuRiC0AgK7EFgBAR2ILYIIcnoLpEFsA0JEwRmwB\nAHQktgAAOhJbAAAdiS0AgI7EFgBAR2ILYGK8ug2mRWwBAHQktgAAOhJbAAAdXdj3AJi2szx3pLW2\ng5EAwDiJLda27pNzj/+8+AJgScQWZ7LNVz+tXpbwAmDuxBZX1Psl5oeXL7oAmCtPkOdEVbXT9/Lx\nvkEAzJXY4gX2FT67DjyYKjPBMC1ii+eMJXbGMAYA2BaxxSgJLgDmQmwxmhmt48Y4JgBYl9hauLEH\nzdjHBwCnEVuMnuACpswLGhBbCzaliJnSWAFgldhaqCnGyxTHDL2YLYHpEFsLJFoAYHfEFpMiFAGY\nGrG1MHOIlTlcB9gGhxLHz21EIrYAALoSWwAAHYmtBZnT4bc5XRfYhMNU4+W24ZDYAgDoSGwthJkg\nANgPsQUwcQ5XjY/bhFVii8kyWwfAFIgtgBkwkzIebguOE1sAAB2JLYCZMKOyf24DTiK2AAA6ElsA\nM2JmZX+sey5HbAEAdCS2AGbGDMvuWedcidgCgA0ILU4jtgBmSADAeIgtgJkSXP1Zx5yF2GKy7OTg\ndO4n/Vi3nJXYApg5UbB91inrEFsLYccAAPshtgAWwB9c22Ndsi6xtSBz2kHM6brArrjfbM465DxO\nja2quruqnqyqh1aW/XpVPTB8faWqHhiWv7yq/nTle7+8cp6bq+rBqrpYVe+rqupzlQC4HLFwftYd\n53XhDD/z/iS/mOSDhwtaa3//8HRVvTfJ/175+UdbazedcDl3Jnl7kt9L8ltJbkvysfWHDMAmWmvx\n9+56hBabOHVmq7X2qSRfP+l7w+zUm5J86EqXUVVXJ3lxa+2+drDFfjDJG9cfLpuaww5jDtcB9s39\n6OysKza16XO2XpPkidbaF1eWXT8cQvzdqnrNsOyaJI+t/Mxjw7ITVdUdVXV/Vd2/4fiYGTs92B73\np9NZR2zDWQ4jXsmbc3RW6/Ek39Va++OqujnJb1bVq9a90NbaXUnuSpKqsqVvmUMIwKHDmLBPOEpk\nsU3njq2qupDkR5PcfListfZ0kqeH05+pqkeTvDLJpSTXrpz92mEZezLF4LLzg36muE/oxb6Gbdvk\nMOLfSfL51tpzhwer6jur6qrh9CuS3JDkS621x5M8VVWvHp7n9ZYkH93gdwOwZSLDOqCPs7z1w4eS\n/I8k311Vj1XV24Zv3Z4XPjH+e5N8bngriP+c5Cdaa4dPrn9Hkv+Q5GKSR+OViHs3lZ1Ka20yY4Wp\nW/J9bcnXnb5q7BvXOs/ZGvt1GasxHzpwm8L+jHnfsE32M8uxzjbdWtvaHcA7yGNHA5xo7rPKc79+\njIfYIsn4gstOEMZjjvfFOV4nxmvTt35gRsbyEnA7QRif1fvlvvcR52Xfwr6Y2eIF9rlDsjOE8Zva\nzPPUxsv8mNniRLue5bIjhOkZy2z45divMBZiiyvqvTO1M4TpO34/3ld82Z8wVmKLM9l2dNkpwnzt\n8vld9iVMgdhiLef5C9bOEJbrpPv/eQPMvoSpEltsxM5vXMb63JltsK3Nh9uSpfFqRACAjsQWMHpm\nQoApE1sAAB2JLZgRM0AA4yO2AAA6ElsAAB2JLQCAjsQWMGqehwZMndiCmREnAOMitoDREo7AHIgt\nmCGRAjAeYgsYJcEIzIXYgpkSKwDjILaA0RGKwJyILZgx0QKwf2ILZm5qwTW18QKcRmzBAkwlYKYy\nToB1iC1YiLGHzNjHB3BeYgsWZKxBM9ZxAWyD2IKFGVvYjG08ANsmtmCBxhI4YxkHQE8X9j0AYD8O\nQ6eq9va7AZbAzBYs3K7DR2gBS2NmC9jJLJfIApZKbAHP2XZ0CSwAsQWcYDWS1g0vgQVwlNgCrkg8\nAWzGE+QBADoSWwAAHYktAICOxBYAQEdiCwCgI7EFANCR2AIA6EhsAQB0JLYAADoSWwAAHYktAICO\nxBYAQEdiCwCgI7EFANCR2AIA6OjCvgewTVW17yEAABxhZgsAoCOxBQDQkdgCAOhIbAEAdCS2AAA6\nElsAAB2JLQCAjk6Nraq6rqp+p6oeqaqHq+qfDsu/o6o+XlVfHP59ycp53l1VF6vqC1X12pXlN1fV\ng8P33lfeGAsAmLmzzGw9k+Sft9ZuTPLqJO+sqhuTvCvJJ1trNyT55PD/DN+7PcmrktyW5Jeq6qrh\nsu5M8vYkNwxft23xugAAjM6psdVae7y19tnh9P9J8gdJrknyhiQfGH7sA0neOJx+Q5IPt9aebq19\nOcnFJLdW1dVJXtxau6+11pJ8cOU8AACztNbH9VTVy5P89SS/l+RlrbXHh2/9UZKXDaevSXLfytke\nG5b92XD6+PKTfs8dSe4Y/vt0kofWGefMvTTJ/9r3IEbE+jjK+jjK+jjK+jjK+niedXHUd2/zws4c\nW1X155OUYlvxAAAFeUlEQVT8lyQ/1Vp7avXpVq21VlVtW4Nqrd2V5K7h997fWrtlW5c9ddbHUdbH\nUdbHUdbHUdbHUdbH86yLo6rq/m1e3plejVhV35SD0Pq11tp/HRY/MRwazPDvk8PyS0muWzn7tcOy\nS8Pp48sBAGbrLK9GrCS/muQPWmv/buVb9yZ563D6rUk+urL89qp6UVVdn4Mnwn96OOT4VFW9erjM\nt6ycBwBgls5yGPFvJfmxJA9W1QPDsn+Z5N8kuaeq3pbkq0nelCSttYer6p4kj+TglYzvbK09O5zv\nHUnen+Rbknxs+DrNXWe7KothfRxlfRxlfRxlfRxlfRxlfTzPujhqq+ujDl4YCABAD95BHgCgI7EF\nANDRaGOrqm4bPu7nYlW9a9/j2YUrfDTSe6rqUlU9MHy9buU8J3400lxU1VeGj3h64PCluOf5qKg5\nqKrvXtkGHqiqp6rqp5a0fVTV3VX1ZFU9tLJssR8ddpn18fNV9fmq+lxVfaSqvn1Y/vKq+tOV7eSX\nV84z5/Wx9v1j5uvj11fWxVcOn4s99+3jCo+vu9l/tNZG95XkqiSPJnlFkm9O8j+T3Ljvce3gel+d\n5G8Mp78tyR8muTHJe5L8ixN+/sZh3bwoyfXDOrtq39djy+vkK0leemzZzyV513D6XUn+7VLWx8o6\nuCoHbyb8V5a0fST53iR/I8lDm2wPST6dg48fqxy8UOeH9n3dtrg+fjDJheH0v11ZHy9f/bljlzPn\n9bH2/WPO6+PY99+b5F8tYfvI5R9fd7L/GOvM1q1JLrbWvtRa+39JPpyDjwGatXb5j0a6nBM/Gqn/\nSPdurY+K2sP4duH7kzzaWvvqFX5mduujtfapJF8/tnixHx120vporf12a+2Z4b/35ej7G77A3NfH\nFSxy+zg0zMa8KcmHrnQZc1kfV3h83cn+Y6yxdU2Sr638/7If7TNXdfSjkZLkJ4fDAnevTHMuYT21\nJJ+oqs/Uwcc4JVf+qKi5r49Dt+foTnKp20ey/vZwTc740WEz8I9z9C12rh8OEf1uVb1mWLaE9bHO\n/WMJ6yNJXpPkidbaF1eWLWL7OPb4upP9x1hja9Hq2EcjJbkzB4dUb0ryeA6mfpfie1prNyX5oSTv\nrKrvXf3m8JfFot6/pKq+Ocnrk/zGsGjJ28cRS9weLqeqfiYH73X4a8Oix5N813B/+mdJ/lNVvXhf\n49sh94+TvTlH/2BbxPZxwuPrc3ruP8YaW5f7yJ/ZqxM+Gqm19kRr7dnW2jeS/EqePxQ0+/XUWrs0\n/Ptkko/k4Lqv+1FRc/NDST7bWnsiWfb2MfDRYcdU1T9M8sNJ/sHwAJLhcMgfD6c/k4PnoLwyM18f\n57h/zHp9JElVXUjyo0l+/XDZEraPkx5fs6P9x1hj6/eT3FBV1w9/xd+eg48BmrXhGPoLPhrpcEMY\n/EiSw1eWnPjRSLsab29V9a1V9W2Hp3PwxN+HsuZHRe121Dtx5C/SpW4fK3x02Iqqui3JTyd5fWvt\nT1aWf2dVXTWcfkUO1seXFrA+1rp/zH19DP5Oks+31p47HDb37eNyj6/Z1f5j168IOOtXktfl4NUC\njyb5mX2PZ0fX+XtyMIX5uSQPDF+vS/Ifkzw4LL83ydUr5/mZYR19IRN8hcgp6+MVOXg1yP9M8vDh\ndpDkLyb5ZJIvJvlEku9YwvoYrt+3JvnjJH9hZdlito8cRObjSf4sB8+VeNt5tockt+TgQffRJL+Y\n4dM0pvZ1mfVxMQfPNTnch/zy8LN/b7gfPZDks0n+7kLWx9r3jzmvj2H5+5P8xLGfnfX2kcs/vu5k\n/+HjegAAOhrrYUQAgFkQWwAAHYktAICOxBYAQEdiCwCgI7EFANCR2AIA6Oj/A8hYUbaba5SwAAAA\nAElFTkSuQmCC\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x16e72f261d0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def Draw_map2():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.rectangle((1500,1000,1600,1100),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,600,900),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,500,600),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1800,800),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1600,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,960,1460),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,500,1800),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,300,1300),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1300,450),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 950, 1900), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,850,300),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1900,330),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "im_show=Draw_map2()\n",
    "plt.imshow(im_show)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map3():\n",
    "    im = Image.new(\"RGB\", size=(2000,2000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB') \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))  \n",
    "    draw.polygon((1150, 1180, 1200, 1180, 1250, 1120, 1230, 1090, 1130, 1100),(0,0,0),(0,0,0))\n",
    "    draw.polygon((400,450,600,750,500,800),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1500,750,1700,950),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((300,1500,1000,1750),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((1450,1600,1600,1750),(0,0,0),(0,0,0))\n",
    "    draw.rectangle((1200,300,1400,500),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((500,1000,700,1200),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((750,900,700,1200),(0,0,0),(0,0,0))\n",
    "    return im\n",
    "# im_show=Draw_map3()\n",
    "# plt.imshow(im_show)\n",
    "# plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Function Scan is used to detect the distance between obstacles and robot.\n",
    "#Nearest(<50cm)||Near(50cm-130cm)||Medium(130cm-210cm)|Far(>210cm)\n",
    "#Furthest scanning distance is defined as 290cm\n",
    "def Scan(Current_x,Current_y,Angle,im):\n",
    "    Distance_level=0\n",
    "    Obstacle_distance=Robot_radium\n",
    "    Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "    Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    while(im.getpixel((Obstacle_distance_x,Obstacle_distance_y))!=(0,0,0) and Obstacle_distance<250):#getpixiel obtains the degree of Gray Scale\n",
    "        Obstacle_distance+=5  #Search interval, can be changed\n",
    "        Obstacle_distance_x=Current_x+Obstacle_distance*np.cos(Angle/180*np.pi)\n",
    "        Obstacle_distance_y=Current_y+Obstacle_distance*np.sin(Angle/180*np.pi)\n",
    "    if 0<=Obstacle_distance<90:\n",
    "        Distance_level=0 #Nearear\n",
    "    elif 90<=Obstacle_distance<170:\n",
    "        Distance_level=1 #Near\n",
    "    elif 170<=Obstacle_distance<250:\n",
    "        Distance_level=2 #Medium\n",
    "    else:\n",
    "        Distance_level=3 #Far\n",
    "    return Distance_level"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Distance=np.arange(0,50,5)\n",
    "    for i in Distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if im.getpixel((x,y))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Random_start(im):\n",
    "    Angle=np.random.random()*360\n",
    "    x,y=np.random.random(2)*2000\n",
    "    while(Is_Crash(x,y,im)==True):\n",
    "        x,y=np.random.random(2)*2000\n",
    "    return x,y,Angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Direction_min_level(Degree,Current_x,Current_y,Current_angle,im):\n",
    "    Level=[]\n",
    "    Degree=Degree+Current_angle\n",
    "    for i in Degree:\n",
    "        Level.append(Scan(Current_x,Current_y,i,im))\n",
    "    return min(Level)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(Left_min,Right_min,Up_min):\n",
    "    LRU=[]\n",
    "    LRU.append(Left_min)\n",
    "    LRU.append(Right_min)\n",
    "    LRU.append(Up_min)\n",
    "    return LRU[0]*16+LRU[1]*4+LRU[2]\n",
    "# print(Output_state_index(Left_min,Right_min,Up_min))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Choose_action is used to selection an action during the training process. It is based on the greedy strategy, if the random \n",
    "#chosen float(0-1) is inferior to current epsilon, robot choose random action to explore, if not, choose maximun Q value\n",
    "#action based on Q Table, more precisely based on the action-state range\n",
    "def Choose_action(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Q_table1_actions_length)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_next_state(Current_x,Current_y,Current_angle,Action,im):\n",
    "    Reward=1\n",
    "    Crash=False\n",
    "    if Action==0:\n",
    "        Next_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "        else:\n",
    "            Reward=2\n",
    "    elif Action==1:\n",
    "        Next_x=Current_x-Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y-Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "            Reward=-500\n",
    "    elif Action==2:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle-45\n",
    "    elif Action==3:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle+45        \n",
    "    return Next_x,Next_y,Next_angle,Reward,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x,Vec_y):\n",
    "    im=Image.new(\"RGB\", size=(2000,2000),color=(0,0,0))\n",
    "    draw = ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0),(0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0),(0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200),(0,0,0),(0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0),(0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.polygon((150, 180, 200, 180, 250, 120, 230, 90, 130, 100),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((Vec_x[0]-40,Vec_y[0]-40, Vec_x[0]+40,Vec_y[0]+40),(0,255,0),(0,255,0)) \n",
    "    for i,j in zip(Vec_x[1:],Vec_y[1:]):\n",
    "        draw.ellipse((i-40,j-40, i+40,j+40),(0,255,0),(0,255,0)) \n",
    "    return im"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-Table!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table=Initial_Q_Table(Q_table1_actions_length,Q_table1_states_length)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval\n",
    "Epoche_action_interval=[]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Start x=149.251874  y=268.432879\n",
      "***********************************************************\n",
      "Epoche=0\n",
      "Epsilon=0.999994\n",
      "***********************************************************\n",
      "Epoche=10\n",
      "Epsilon=0.993919\n",
      "***********************************************************\n",
      "Epoche=20\n",
      "Epsilon=0.979915\n",
      "***********************************************************\n",
      "Epoche=30\n",
      "Epsilon=0.963360\n",
      "***********************************************************\n",
      "Epoche=40\n",
      "Epsilon=0.949025\n",
      "***********************************************************\n",
      "Epoche=50\n",
      "Epsilon=0.943323\n",
      "***********************************************************\n",
      "Epoche=60\n",
      "Epsilon=0.938272\n",
      "***********************************************************\n",
      "Epoche=70\n",
      "Epsilon=0.935330\n",
      "***********************************************************\n",
      "Epoche=80\n",
      "Epsilon=0.931880\n",
      "***********************************************************\n",
      "Epoche=90\n",
      "Epsilon=0.929213\n",
      "***********************************************************\n",
      "Epoche=100\n",
      "Epsilon=0.923923\n",
      "***********************************************************\n",
      "Epoche=110\n",
      "Epsilon=0.920330\n",
      "***********************************************************\n",
      "Epoche=120\n",
      "Epsilon=0.917431\n",
      "***********************************************************\n",
      "Epoche=130\n",
      "Epsilon=0.913278\n",
      "***********************************************************\n",
      "Epoche=140\n",
      "Epsilon=0.908382\n",
      "***********************************************************\n",
      "Epoche=150\n",
      "Epsilon=0.903953\n",
      "***********************************************************\n",
      "Epoche=160\n",
      "Epsilon=0.900195\n",
      "***********************************************************\n",
      "Epoche=170\n",
      "Epsilon=0.896486\n",
      "***********************************************************\n",
      "Epoche=180\n",
      "Epsilon=0.894198\n",
      "***********************************************************\n",
      "Epoche=190\n",
      "Epsilon=0.890984\n",
      "***********************************************************\n",
      "Epoche=200\n",
      "Epsilon=0.887556\n",
      "***********************************************************\n",
      "Epoche=210\n",
      "Epsilon=0.882831\n",
      "***********************************************************\n",
      "Epoche=220\n",
      "Epsilon=0.880700\n",
      "***********************************************************\n",
      "Epoche=230\n",
      "Epsilon=0.876802\n",
      "***********************************************************\n",
      "Epoche=240\n",
      "Epsilon=0.873903\n",
      "***********************************************************\n",
      "Epoche=250\n",
      "Epsilon=0.870806\n",
      "***********************************************************\n",
      "Epoche=260\n",
      "Epsilon=0.867216\n",
      "***********************************************************\n",
      "Epoche=270\n",
      "Epsilon=0.865875\n",
      "***********************************************************\n",
      "Epoche=280\n",
      "Epsilon=0.862622\n",
      "***********************************************************\n",
      "Epoche=290\n",
      "Epsilon=0.861053\n",
      "***********************************************************\n",
      "Epoche=300\n",
      "Epsilon=0.859361\n",
      "***********************************************************\n",
      "Epoche=310\n",
      "Epsilon=0.857060\n",
      "***********************************************************\n",
      "Epoche=320\n",
      "Epsilon=0.854649\n",
      "***********************************************************\n",
      "Epoche=330\n",
      "Epsilon=0.853411\n",
      "***********************************************************\n",
      "Epoche=340\n",
      "Epsilon=0.851311\n",
      "***********************************************************\n",
      "Epoche=350\n",
      "Epsilon=0.848805\n",
      "***********************************************************\n",
      "Epoche=360\n",
      "Epsilon=0.846784\n",
      "***********************************************************\n",
      "Epoche=370\n",
      "Epsilon=0.844820\n",
      "***********************************************************\n",
      "Epoche=380\n",
      "Epsilon=0.843469\n",
      "***********************************************************\n",
      "Epoche=390\n",
      "Epsilon=0.840895\n",
      "***********************************************************\n",
      "Epoche=400\n",
      "Epsilon=0.839384\n",
      "***********************************************************\n",
      "Epoche=410\n",
      "Epsilon=0.837272\n",
      "***********************************************************\n",
      "Epoche=420\n",
      "Epsilon=0.834627\n",
      "***********************************************************\n",
      "Epoche=430\n",
      "Epsilon=0.832698\n",
      "***********************************************************\n",
      "Epoche=440\n",
      "Epsilon=0.830268\n",
      "***********************************************************\n",
      "Epoche=450\n",
      "Epsilon=0.828586\n",
      "***********************************************************\n",
      "Epoche=460\n",
      "Epsilon=0.827388\n",
      "***********************************************************\n",
      "Epoche=470\n",
      "Epsilon=0.825771\n",
      "***********************************************************\n",
      "Epoche=480\n",
      "Epsilon=0.823917\n",
      "***********************************************************\n",
      "Epoche=490\n",
      "Epsilon=0.821279\n",
      "***********************************************************\n",
      "Epoche=500\n",
      "Epsilon=0.819586\n",
      "***********************************************************\n",
      "Epoche=510\n",
      "Epsilon=0.817926\n",
      "***********************************************************\n",
      "Epoche=520\n",
      "Epsilon=0.815696\n",
      "***********************************************************\n",
      "Epoche=530\n",
      "Epsilon=0.813751\n",
      "***********************************************************\n",
      "Epoche=540\n",
      "Epsilon=0.811242\n",
      "***********************************************************\n",
      "Epoche=550\n",
      "Epsilon=0.810131\n",
      "***********************************************************\n",
      "Epoche=560\n",
      "Epsilon=0.807465\n",
      "***********************************************************\n",
      "Epoche=570\n",
      "Epsilon=0.806494\n",
      "***********************************************************\n",
      "Epoche=580\n",
      "Epsilon=0.804556\n",
      "***********************************************************\n",
      "Epoche=590\n",
      "Epsilon=0.803221\n",
      "***********************************************************\n",
      "Epoche=600\n",
      "Epsilon=0.800558\n",
      "***********************************************************\n",
      "Epoche=610\n",
      "Epsilon=0.798921\n",
      "***********************************************************\n",
      "Epoche=620\n",
      "Epsilon=0.796883\n",
      "***********************************************************\n",
      "Epoche=630\n",
      "Epsilon=0.794506\n",
      "***********************************************************\n",
      "Epoche=640\n",
      "Epsilon=0.792172\n",
      "***********************************************************\n",
      "Epoche=650\n",
      "Epsilon=0.790952\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-54-454d730b9ac3>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m     69\u001b[0m     \u001b[1;32mreturn\u001b[0m \u001b[0mQ_Table\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     70\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 71\u001b[1;33m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mRun1\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     72\u001b[0m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mpd\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mDataFrame\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mcolumns\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mQ_table1_actions\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mindex\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mQ_table1_states\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     73\u001b[0m \u001b[0mQ_Table_Final\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32m<ipython-input-54-454d730b9ac3>\u001b[0m in \u001b[0;36mRun1\u001b[1;34m()\u001b[0m\n\u001b[0;32m     21\u001b[0m     \u001b[1;32mwhile\u001b[0m \u001b[0mEpoche_false\u001b[0m\u001b[1;33m<\u001b[0m\u001b[1;36m5000\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     22\u001b[0m         \u001b[0mCurrent_left_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mLeft_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 23\u001b[1;33m         \u001b[0mCurrent_right_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mRight_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     24\u001b[0m         \u001b[0mCurrent_up_obstacle_level\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mUp_degree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     25\u001b[0m         \u001b[0mCurrent_state\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mOutput_state_index\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mCurrent_left_obstacle_level\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_right_obstacle_level\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_up_obstacle_level\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32m<ipython-input-48-ba6efb9753b3>\u001b[0m in \u001b[0;36mDirection_min_level\u001b[1;34m(Degree, Current_x, Current_y, Current_angle, im)\u001b[0m\n\u001b[0;32m      1\u001b[0m \u001b[1;32mdef\u001b[0m \u001b[0mDirection_min_level\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mDegree\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      2\u001b[0m     \u001b[0mLevel\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 3\u001b[1;33m     \u001b[0mDegree\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mDegree\u001b[0m\u001b[1;33m+\u001b[0m\u001b[0mCurrent_angle\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      4\u001b[0m     \u001b[1;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[1;32min\u001b[0m \u001b[0mDegree\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m         \u001b[0mLevel\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mappend\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mScan\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mCurrent_x\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mCurrent_y\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mi\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mim\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "def Run1():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Epoche_action_interval\n",
    "    Crash=False\n",
    "    Choose_map=np.random.randint(3)\n",
    "    if Choose_map==0:\n",
    "        im=Draw_map1()\n",
    "    elif Choose_map==1:\n",
    "        im=Draw_map2()\n",
    "    else:\n",
    "        im=Draw_map3()\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    print('Start x=%f  y=%f'%(Current_x,Current_y))\n",
    "    Epoche_false=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    Vec_x.append(Current_x)\n",
    "    Vec_y.append(Current_y)\n",
    "    Epoche_action_times=0\n",
    "    while Epoche_false<5000:\n",
    "        Current_left_obstacle_level=Direction_min_level(Left_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_right_obstacle_level=Direction_min_level(Right_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_obstacle_level=Direction_min_level(Up_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state=Output_state_index(Current_left_obstacle_level,Current_right_obstacle_level,Current_up_obstacle_level)\n",
    "        \n",
    "        Next_action=Choose_action(Q_Table,Current_state,Action_times)\n",
    "#         print('Current_x=%f  Current_y=%f  Current_angle=%f Current_action=%d'%(Current_x,Current_y,Current_angle,Next_action))\n",
    "        Next_x,Next_y,Next_angle,Reward,Crash=Output_next_state(Current_x,Current_y,Current_angle,Next_action,im)\n",
    "        \n",
    "        if Crash==True:\n",
    "            Q_target=Reward\n",
    "            if(Epoche%10==0):\n",
    "                print('***********************************************************')\n",
    "                print('Epoche=%d'%Epoche)\n",
    "                print(\"Epsilon=%f\"%(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)))\n",
    "                Movement_plot(Vec_x,Vec_y)\n",
    "                Vec_x=[]\n",
    "                Vec_y=[]\n",
    "            Epoche+=1\n",
    "            Epoche_false+=1\n",
    "            Choose_map=np.random.randint(3)\n",
    "            if Choose_map==0:\n",
    "                im=Draw_map1()\n",
    "            elif Choose_map==1:\n",
    "                im=Draw_map2()\n",
    "            else:\n",
    "                im=Draw_map3()\n",
    "            Next_x,Next_y,Next_angle=Random_start(im)\n",
    "            Epoche_action_interval.append(Epoche_action_times)\n",
    "            Epoche_action_times=0\n",
    "        \n",
    "        else:\n",
    "            Next_left_obstacle_level=Direction_min_level(Left_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_right_obstacle_level=Direction_min_level(Right_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_up_obstacle_level=Direction_min_level(Up_degree,Next_x,Next_y,Next_angle,im)\n",
    "            Next_state=Output_state_index(Next_left_obstacle_level,Next_right_obstacle_level,Next_up_obstacle_level)\n",
    "            Q_target=Reward+Beta*max(Q_Table[Next_state])\n",
    "            \n",
    "        Q_Table[Current_state][Next_action]+=Alpha*(Q_target-Q_Table[Current_state][Next_action])\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_a=Next_angle\n",
    "        Vec_x.append(Current_x)\n",
    "        Vec_y.append(Current_y)\n",
    "        Action_times+=1\n",
    "        Epoche_action_times+=1\n",
    "    plt.plot(np.arange(len(Epoche_action_interval)),Epoche_action_interval)\n",
    "    return Q_Table\n",
    "print()\n",
    "Q_Table_Final=Run1()\n",
    "Q_Table_Final=pd.DataFrame(Q_Table_Final,columns=Q_table1_actions,index=Q_table1_states)\n",
    "Q_Table_Final\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def test(Q_Table):\n",
    "    Crash=False\n",
    "    im=Draw_map1()\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    Test_time=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    while Test_time<100001:\n",
    "        Current_left_obstacle_level=Direction_min_level(Left_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_right_obstacle_level=Direction_min_level(Right_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_obstacle_level=Direction_min_level(Up_degree,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state=Output_state_index(Current_left_obstacle_level,Current_right_obstacle_level,Current_up_obstacle_level)\n",
    "        Next_action=np.argmax(Q_Table[Current_state])\n",
    "#         print(\"------------------------------------------------------------------------------------------\")\n",
    "#         print('Current_x=%f   Current_y=%f   Current_angle=%f'%(Current_x,Current_y,(Current_angle%360)))\n",
    "        Next_x,Next_y,Next_angle,Reward,Crash=Output_next_state(Current_x,Current_y,Current_angle,Next_action,im)\n",
    "#         print('Current_state=%d'%Current_state)\n",
    "#         print('Next_action=%d'%Next_action)\n",
    "#         print('Next_x=%f   Next_y=%f   Next_angle=%f'%(Next_x,Next_y,(Next_angle%360)))\n",
    "        \n",
    "        Vec_x.append(Current_x)\n",
    "        Vec_y.append(Current_y)\n",
    "        if Crash==True:\n",
    "            print('Boom')\n",
    "            print(Test_time)\n",
    "            break\n",
    "        else:\n",
    "            if(Test_time%10000==0):\n",
    "                print(\"******************\")\n",
    "                print(Test_time)\n",
    "                im_show=Movement_plot(Vec_x,Vec_y)\n",
    "                print(\"******************\")\n",
    "                plt.imshow(im_show)\n",
    "                plt.show()\n",
    "                Vec_x=[]\n",
    "                Vec_y=[]\n",
    "                Next_x,Next_y,Current_a=Random_start(im)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "        Test_time+=1\n",
    "test(Q_Table_Final.as_matrix())\n",
    "# np.savetxt(\"Q_Table_try.txt\",Q_Table_Final.as_matrix())\n",
    "            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table_Final.iloc[30,:]"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
